﻿using UnityEngine;
using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;




public interface IBoid
{
    Vector3 velocity { get; set; }
    Vector3 position { get; set; }

    //float mass { get; set; }

    //float MAX_VELOCITY { get; set; }
}



public class SteeringMgr
{
    public Vector3 steering;

    public IBoid host;


    public float MAX_VELOCITY;

    //public float MAX_FORCE = 100.0f;

    /// The constructor
    public SteeringMgr(IBoid host, float MAX_VELOCITY)
    {
        this.host = host;
        this.steering = Vector3.zero;
        this.MAX_VELOCITY = MAX_VELOCITY;
    }

    #region The public API (one method for each behavior)
    public void seek(Vector3 target) { steering += doSeek(target); }
    public void flee(Vector3 target) { steering += doFlee(target); }
    public void arrival(Vector3 target, float slowingRadius) { steering += doArrival(target, slowingRadius); }

    public void collisionAvoidance(float MAX_SEE_AHEAD, float MAX_AVOID_FORCE, List<IBoid> obstacles, float radius) { steering += doCollisionAvoidance(MAX_SEE_AHEAD, MAX_AVOID_FORCE, obstacles, radius); }

    //public function wander() :void {}
    //public function evade(target :IBoid) :void {}
    //public function pursuit(target :IBoid) :void {}
    #endregion

    #region The internal API
    private Vector3 doSeek(Vector3 target)
    {
        Vector3 force = Vector3.zero;

        var desired_velocity = (target - host.position).normalized * MAX_VELOCITY;

        force = desired_velocity - host.velocity;

        return force;
    }
    private Vector3 doFlee(Vector3 target)
    {
        return -doSeek(target);
    }
    /// The real implementation of seek (with arrival code included)
    private Vector3 doArrival(Vector3 target, float slowingRadius)
    {
        Vector3 force = Vector3.zero;

        var desired_velocity = target - host.position;
        var distance = desired_velocity.magnitude;
        if (distance <= slowingRadius)
        {
            desired_velocity = desired_velocity.normalized * MAX_VELOCITY * (distance / slowingRadius);
        }
        else
        {
            desired_velocity = desired_velocity.normalized * MAX_VELOCITY;
        }

        force = desired_velocity - host.velocity;

        return force;
    }
    private Vector3 doCollisionAvoidance(float MAX_SEE_AHEAD, float MAX_AVOID_FORCE, List<IBoid> obstacles, float radius)
    {
        Vector3 ahead = host.position + host.velocity.normalized * MAX_SEE_AHEAD;
        Vector3 ahead2 = host.position + host.velocity.normalized * MAX_SEE_AHEAD * 0.5f;

        Debug.DrawLine(host.position, ahead, Color.red);

        IBoid mostThreatening = findMostThreateningObstacle(ahead, ahead2, obstacles, radius);

        Vector3 avoidance = Vector3.zero;

        if (mostThreatening != null)
        {
            //Debug.Log(selfControl.transform.name + " find  mostThreatening :" + mostThreatening.transform.name);
            avoidance = (ahead - mostThreatening.position).normalized * MAX_AVOID_FORCE;

            if (Mathf.Approximately(180.0f, Vector3.Angle(host.velocity, avoidance)))
            {
                avoidance = UnityEngine.Random.Range(0, 2) == 0 ? new Vector3(-avoidance.y, avoidance.x, 0) : new Vector3(avoidance.y, -avoidance.x, 0);
            }
        }

        return avoidance;
    }

    //private function doWander() :Vector3D {}
    //private function doEvade(target :IBoid) :Vector3D {}
    //private function doPursuit(target :IBoid) :Vector3D {}
    #endregion


    /// The update method. 
    /// Should be called after all behaviors have been invoked
    public void update()
    {
        //steering = nothing(); // the null vector, meaning "zero force magnitude"
        //steering = steering + seek(); // assuming the character is seeking something
        //steering = steering + collisionAvoidance();

        //steering = truncate(steering, MAX_FORCE);
        //steering /= host.mass;

        host.velocity = truncate(host.velocity + steering, MAX_VELOCITY);
        host.position += host.velocity * Time.deltaTime;
    }

    /// Reset the internal steering force.
    public void reset()
    {
        steering = Vector3.zero;
    }

    #region Util
    private IBoid findMostThreateningObstacle(Vector3 ahead, Vector3 ahead2, List<IBoid> obstacles, float radius)
    {
        IBoid mostThreatening = null;

        foreach (IBoid curObstacle in obstacles)
        {
            if (curObstacle == host) continue;

            bool collision = lineIntersectsCircle(ahead, ahead2, host.position, curObstacle.position, radius);

            if (collision && (mostThreatening == null || Vector3.SqrMagnitude(host.position - curObstacle.position) < Vector3.SqrMagnitude(host.position - mostThreatening.position)))
            {
                mostThreatening = curObstacle;
            }
        }

        return mostThreatening;
    }
    private bool lineIntersectsCircle(Vector3 ahead, Vector3 ahead2, Vector3 curPos, Vector3 center, float radius)
    {
        return Vector3.SqrMagnitude(ahead - center) < (radius * radius) || Vector3.SqrMagnitude(ahead2 - center) < (radius * radius) || Vector3.SqrMagnitude(curPos - center) < (radius * radius);
    }

    private Vector3 truncate(Vector3 vector, float max)
    {
        float i = max / vector.magnitude;

        i = i < 1.0f ? i : 1.0f;

        vector *= i;

        return vector;
    }
    #endregion
}


